#include "libmesh/quadrature.h"
#include "Assembly.h"
#include "CFDForceUserObject.h"
#include "CFDProblem.h"

using Eigen::Quaterniond;

template<>
InputParameters validParams<CFDForceUserObject>()
{
	InputParameters params = validParams<SideUserObject>();
	params += validParams<MultiVariableInterface>();

	params.addParam<Real>("ref_area"  , 1, "参考面积");
	params.addParam<Point>("ref_point", "力距参考点");
	params.addParam<Real>("ref_length", "参考长度");
	return params;
}

CFDForceUserObject::CFDForceUserObject(const InputParameters &parameters) :
	SideUserObject(parameters),
	MultiVariableInterface(parameters),
	_cfd_problem(static_cast<CFDProblem&>(_fe_problem)),
	_cfd_data(_cfd_problem),
	_qp(0),
	_momentum(isParamValid("ref_point") ? true : false),
	_force   (isParamValid("ref_area")  ? true : false)
{
	if(_force)
		_ref_area = getParam<Real>("ref_area");
	if(_momentum)
	{
		Point ref_point = getParam<Point>("ref_point");
		 _ref_point = Vector3d(ref_point(0), ref_point(1), ref_point(2));
	}

	// foreach(MooseVariable *var, _variables)
	for(auto var : _variables)
		addMooseVariableDependency(var);
}

void CFDForceUserObject::initialize()
{
	_form_force.setZero();
	_friction_force.setZero();
}

void CFDForceUserObject::execute()
{
	for (_qp = 0; _qp < _qrule->n_points(); _qp++)
	{
		reinit();

		Real pre = _cfd_data.p;
		RealTensor &tau = _cfd_data.tau;;
		RealVectorValue normal = -_normals[_qp];

		RealVectorValue form_force, friction_force, total_force;
		form_force = -pre*normal/(0.5*_ref_area);
		friction_force = tau*normal/(0.5*_ref_area);

		_form_force(0) 	   += _JxW[_qp]*_coord[_qp]*form_force(0);
		_form_force(1)     += _JxW[_qp]*_coord[_qp]*form_force(1);
		_form_force(2)     += _JxW[_qp]*_coord[_qp]*form_force(2);

		_friction_force(0) += _JxW[_qp]*_coord[_qp]*friction_force(0);
		_friction_force(1) += _JxW[_qp]*_coord[_qp]*friction_force(1);
		_friction_force(2) += _JxW[_qp]*_coord[_qp]*friction_force(2);
	}
}

void CFDForceUserObject::threadJoin(const UserObject& uo)  // 线程间求和
{
	const CFDForceUserObject & pps = static_cast<const CFDForceUserObject &>(uo);
	_form_force += pps._form_force;
	_friction_force += pps._friction_force;
}

void CFDForceUserObject::finalize()
{
	gatherSum(_form_force(0));
	gatherSum(_form_force(1));
	gatherSum(_form_force(2));

	gatherSum(_friction_force(0));
	gatherSum(_friction_force(1));
	gatherSum(_friction_force(2));

	Quaterniond q = _cfd_problem._attitude.earthFromWind().inverse();

	 _form_force = q*_form_force;
	 _friction_force = q*_friction_force;
	 _total_force = _form_force + _friction_force;

	 std::ostringstream oss;
	 // Perform the output of the variable residuals
//	 oss << "    |residual|_2 of individual variables:\n";
//	 for (unsigned int var_num = 0; var_num < _sys.n_vars(); var_num++)
//	 {
//		 Real var_res_id = _sys.calculate_norm(*_sys.rhs,var_num,DISCRETE_L2);
//		 oss << std::setw(27-max_name_size) << " " << std::setw(max_name_size+2) //match position of overall NL residual
//		 << std::left << _sys.variable_name(var_num) + ":" << var_res_id << "\n";
//	 }

	int max_name_size = 15;
	oss << COLOR_CYAN << "\n--------------------------Drag && Lift--------------------------" << COLOR_DEFAULT <<"\n\n";
	oss << std::setw(20-max_name_size) << " " << std::setw(max_name_size+2)
	<< std::left << "Form Force: " << _form_force.transpose() << "\n";
	oss << std::setw(20-max_name_size) << " " << std::setw(max_name_size+2)
	<< std::left << "Friction Force: " << _friction_force.transpose() << "\n";
	oss << std::setw(20-max_name_size) << " " << std::setw(max_name_size+2)
	<< std::left << "Total Force: " << _total_force.transpose() << "\n";

	if(_momentum)
	{
		_total_momentum = _total_force.cross(_ref_point);
		_total_momentum = q * _total_momentum;
		oss << std::setw(20-max_name_size) << " " << std::setw(max_name_size+2)
		<< std::left << "Momentun: " << _total_momentum.transpose() << "\n";
	}
	oss << COLOR_CYAN << "\n----------------------------------------------------------------" << COLOR_DEFAULT <<"\n";
	_console << oss.str() <<std::flush;
}

void CFDForceUserObject::reinit()
{
	for (int eq = 0; eq < _n_equation; ++eq)
	{
		_cfd_data.uh[eq] = (*_uh[eq])[_qp];
		_cfd_data.duh[eq] = (*_grad_uh[eq])[_qp];
	}
	_cfd_data.reinit();
}
